// --------------------------------------------------------------------------------------------------------------------
// <copyright file="HomogeneousTransform2D.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Numerics
{
    using System;
    using System.Diagnostics;
    using ShoNS.Array;

    /// <summary>
    /// Class to create and work with 2D homogeneous transforms (stored as 3x3 matrices) between different coordinate frames.
    /// </summary>
    public static class HomogeneousTransform2D
    {
        /// <summary>
        /// Builds a 2D Euclidean transform that rotates by a certain amount.  Assumes that x and y form a right-hand coordinate system, and the rotation is
        /// about the positive z axis (i.e., counterclockwise if x points right and y points up in the x-y plane).
        /// The 3x3 matrix looks like this:
        ///    [  R  t]
        ///    [ 0 0 1]
        /// where R is a 2x2 rotation sub-matrix and t=[0 0]^T is a 2x1 translation vector.
        /// </summary>
        /// <param name="angle">Angle to rotate (radians)</param>
        /// <returns>2D Euclidean transform</returns>
        public static Matrix3 CreateFromRotation(double angle)
        {
            Matrix3 euclidTransform = new Matrix3();
            euclidTransform[0, 0] = Math.Cos(angle);
            euclidTransform[0, 1] = -Math.Sin(angle);
            euclidTransform[0, 2] = 0.0;
            euclidTransform[1, 0] = Math.Sin(angle);
            euclidTransform[1, 1] = Math.Cos(angle);
            euclidTransform[1, 2] = 0.0;
            euclidTransform[2, 0] = 0;
            euclidTransform[2, 1] = 0;
            euclidTransform[2, 2] = 1;
            return euclidTransform;
        }

        /// <summary>
        /// Builds a 2D Euclidean transform that translates by a certain amount.  Assumes that x and y form a right-hand coordinate system, and the rotation is
        /// about the positive z axis (i.e., counterclockwise assuming x points right and y points up in the x-y plane).
        /// The 3x3 matrix looks like this:
        ///    [  R  t]
        ///    [ 0 0 1]
        /// where R=I is a 2x2 rotation sub-matrix and t is a 2x1 translation vector.
        /// </summary>
        /// <param name="tx">Translation in x</param>
        /// <param name="ty">Translation in y</param>
        /// <returns>2D Euclidean transform</returns>
        public static Matrix3 CreateFromTranslation(double tx, double ty)
        {
            Matrix3 euclidTransform = new Matrix3();
            euclidTransform[0, 0] = 1.0;
            euclidTransform[0, 1] = 0.0;
            euclidTransform[0, 2] = tx;
            euclidTransform[1, 0] = 0.0;
            euclidTransform[1, 1] = 1.0;
            euclidTransform[1, 2] = ty;
            euclidTransform[2, 0] = 0;
            euclidTransform[2, 1] = 0;
            euclidTransform[2, 2] = 1;
            return euclidTransform;
        }

        /// <summary>
        /// Builds a 2D Euclidean transform that rotates and translates by a certain amount.  Assumes that x and y form a right-hand coordinate system, and the rotation is
        /// about the positive z axis (i.e., counterclockwise if x points right and y points up in the x-y plane).
        /// The 3x3 matrix looks like this:
        ///    [  R  t]
        ///    [ 0 0 1]
        /// where R is a 2x2 rotation sub-matrix and t is a 2x1 translation vector.
        /// </summary>
        /// <param name="angle">Angle to rotate (radians)</param>
        /// <param name="tx">Translation in x</param>
        /// <param name="ty">Translation in y</param>
        /// <returns>2D Euclidean transform</returns>
        public static Matrix3 CreateFromRotationAndTranslation(double angle, double tx, double ty)
        {
            Matrix3 euclidTransform = new Matrix3();
            euclidTransform[0, 0] = Math.Cos(angle);
            euclidTransform[0, 1] = -Math.Sin(angle);
            euclidTransform[0, 2] = tx;
            euclidTransform[1, 0] = Math.Sin(angle);
            euclidTransform[1, 1] = Math.Cos(angle);
            euclidTransform[1, 2] = ty;
            euclidTransform[2, 0] = 0;
            euclidTransform[2, 1] = 0;
            euclidTransform[2, 2] = 1;
            return euclidTransform;
        }

        /// <summary>
        /// Applies a 2D homogeneous transform to a 2D vector, assuming the transform is affine.  
        /// That is, this method assumes the last row of ht is [0 0 1], though this is not verified.
        /// </summary>
        /// <param name="ht">2D homogenous transform (assumed to also be affine)</param>
        /// <param name="vec">2D vector to be transformed</param>
        /// <returns>Transformed vector.</returns>
        public static Vector2 ApplyAffine(Matrix3 ht, Vector2 vec)
        {
            return new Vector2(
              (ht[0, 0] * vec.X) + (ht[0, 1] * vec.Y) + ht[0, 2],
              (ht[1, 0] * vec.X) + (ht[1, 1] * vec.Y) + ht[1, 2]);
        }

        /// <summary>
        /// Applies a 2D homogeneous transform to a 2D vector, allowing the transform to be non-affine.  
        /// If the transform *is* affine, that is, if the last row of ht is [0 0 1], then the Z coordinate
        /// of the result will be 1.
        /// </summary>
        /// <param name="ht">2D homogenous transform</param>
        /// <param name="vec">2D vector to be transformed</param>
        /// <returns>Transformed vector.</returns>
        public static Vector3 ApplyNonAffine(Matrix3 ht, Vector2 vec)
        {
            return new Vector3(
              (ht[0, 0] * vec.X) + (ht[0, 1] * vec.Y) + ht[0, 2],
              (ht[1, 0] * vec.X) + (ht[1, 1] * vec.Y) + ht[1, 2],
              (ht[2, 0] * vec.X) + (ht[2, 1] * vec.Y) + ht[2, 2]);
        }

        /// <summary>
        /// Applies a 2D homogeneous transform to a 2D homogeneous vector, allowing the transform to be non-affine.  
        /// If the transform *is* affine (i.e., the last row of ht is [0 0 1]) and if the last Z coordinate
        /// of vec is 1, then the Z coordinate of the result will be 1.  This method allows the application of
        /// 2D homogeneous transforms (in the computer vision sense, which includes projective transforms) to 
        /// 2D points/lines in the plane expressed as homogeneous coordinates (in the computer vision sense, 
        /// in which the last coordinate is not restricted to be 1).
        /// </summary>
        /// <param name="ht">2D homogenous transform</param>
        /// <param name="vec">2D vector to be transformed (stored as homogeneous coordinates)</param>
        /// <returns>Transformed vector.</returns>
        public static Vector3 ApplyNonAffine(Matrix3 ht, Vector3 vec)
        {
            return new Vector3(
              (ht[0, 0] * vec.X) + (ht[0, 1] * vec.Y) + ht[0, 2],
              (ht[1, 0] * vec.X) + (ht[1, 1] * vec.Y) + ht[1, 2],
              (ht[2, 0] * vec.X) + (ht[2, 1] * vec.Y) + ht[2, 2]);
        }

        /// <summary>
        /// Builds a 2D similarity (possibly with reflection) transform that best aligns two point sets using the $Procrustes$ algorithm.
        /// The point sets are ordered in the same way, so that the $i$th element of one corresponds to the $i$th element of the other, for all i.
        /// The 3x3 matrix looks like this:
        ///    [ sR  t]
        ///    [ 0 0 1]
        /// where R is a 2x2 rotation/reflection sub-matrix, t is a 2x1 translation vector, and s is a scalar.
        /// The variant of $Procrustes$ used depends on the parameters:
        ///      allowScale = false, allowReflection = false:  constrained orthogonal $Procrustes$
        ///      allowScale = false, allowReflection = true:   orthogonal $Procrustes$
        ///      allowScale = true, allowReflection = false:   constrained $Procrustes$
        ///      allowScale = true, allowReflection = true:    full $Procrustes$ 
        /// </summary>
        /// <param name="points1">First list of 2D points</param>
        /// <param name="points2">Second list of 2D points</param>
        /// <param name="allowScale">Whether to normalize for scale</param>
        /// <param name="allowReflection">Whether to allow for mirror reflection</param>
        /// <returns>2D Similarity transform (possibly with reflection)</returns>
        public static Matrix3 CreateFromProcrustes(Vector2[] points1, Vector2[] points2, bool allowScale, bool allowReflection)
        {
            if (points1.Length != points2.Length)
            {
                throw new ArgumentException("Point lists must have the same length");
            }

            if (points1.Length == 0)
            {
                throw new ArgumentException("Point lists must not be empty");
            }

            // Compute centroids
            Vector2 centroid1 = new Vector2();
            Vector2 centroid2 = new Vector2();
            for (int i = 0; i < points1.Length; i++)
            {
                centroid1 += points1[i];
                centroid2 += points2[i];
            }

            centroid1 /= points1.Length;
            centroid2 /= points1.Length;

            // Subtract centroids
            Vector2[] points1normalized = new Vector2[points1.Length];
            Vector2[] points2normalized = new Vector2[points1.Length];
            for (int i = 0; i < points1.Length; i++)
            {
                points1normalized[i] = points1[i] - centroid1;
                points2normalized[i] = points2[i] - centroid2;
            }

            // Compute scale
            double scale = 1.0;
            if (allowScale)
            {
                double meanDistance1 = 0;
                double meanDistance2 = 0;
                for (int i = 0; i < points1.Length; i++)
                {
                    meanDistance1 += points1normalized[i].L2Norm();
                    meanDistance2 += points2normalized[i].L2Norm();
                }

                meanDistance1 /= points1.Length;
                meanDistance2 /= points1.Length;

                // Normalize for scale
                for (int i = 0; i < points1.Length; i++)
                {
                    points1normalized[i] /= meanDistance1;
                    points2normalized[i] /= meanDistance2;
                }

                scale = meanDistance2 / meanDistance1;
            }

            // Create 2x2 matrix of cross products of shifted and scaled points
            DoubleArray mat = new DoubleArray(2, 2);
            for (int i = 0; i < points1.Length; i++)
            {
                mat[0, 0] += points2normalized[i].X * points1normalized[i].X;
                mat[0, 1] += points2normalized[i].X * points1normalized[i].Y;
                mat[1, 0] += points2normalized[i].Y * points1normalized[i].X;
                mat[1, 1] += points2normalized[i].Y * points1normalized[i].Y;
            }

            // Apply orthogonal Procrustes algorithm, which uses the SVD, to compute the rotation matrix between the shifted point arrays
            SVD svd = new SVD(mat);
            DoubleArray u = svd.U;
            DoubleArray d = svd.D;
            DoubleArray v = svd.V;
            DoubleArray utimesv = u * v.T;
            double det = utimesv.Det();
            d[0, 0] = 1;
            d[1, 1] = (det >= 0) ? 1 : -1;
            DoubleArray rot = (allowReflection) ? u * v.T : u * d * v.T;

            // Convert centroids and rotation matrix to 3x3 homogeneous transform
            Matrix3 transform = new Matrix3();
            transform[0, 0] = scale * rot[0, 0];
            transform[0, 1] = scale * rot[0, 1];
            transform[0, 2] = centroid2.X - scale * rot[0, 0] * centroid1.X - scale * rot[0, 1] * centroid1.Y;
            transform[1, 0] = scale * rot[1, 0];
            transform[1, 1] = scale * rot[1, 1];
            transform[1, 2] = centroid2.Y - scale * rot[1, 0] * centroid1.X - scale * rot[1, 1] * centroid1.Y;
            transform[2, 0] = 0;
            transform[2, 1] = 0;
            transform[2, 2] = 1;
            return transform;
        }
    }
}
